#ifndef RSIM_RIGIDBODYNODE_H
#define RSIM_RIGIDBODYNODE_H

/* -------------------------------------------------------------------------- *
 * File: RigidBodyNode.h                                                      *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "List.h"
#include "RigidBody.h"
#include "Force.h"
#include "State.h"

namespace RSIM{

class SystemAcceleration;
class State;

class RigidBodyNode{
        public:
                /** Pointer to the parent of this node*/
                RigidBodyNode *Parent_;
                
                /** List of children of this node */
                List <RigidBodyNode*> Child_;
                
                /** This is the actual data stored in the node */
                RigidBody *data_;
                
                /** This is the node id */
                int NodeId_;
                
                /** NULL Node */
                RigidBodyNode();
                
                /** clear *data_*/
                ~RigidBodyNode();
                
                /** Create a new child to the *ParentNode with RigidBody data */
                RigidBodyNode(RigidBodyNode *ParentNode, const int& NodeId, const RigidBody& data);
                
                /** Return node id. This is distinct from BodyIndex. */
                int getNodeId() const;
                
                /** This is called by the function Tree::setRoot() */
                RigidBodyNode(const int& NodeId, const RigidBody& data);
                
                void updJointTransform(const double *bodyq);
                
                void sweepBackwardsABA(const int& ChildID, State& state);
                
                void initBetaK(const double *bodyq);
                
                void sweepForwardRNEA(const SystemAcceleration *SysAccl,const int& ChildID,State& state);
                
                void sweepBackwardRNEA(const int& ChildID, State& state);
                
                void updQ(const double *q, State& state)const;
                
                void updU(const double *u, State& state)const;
                
                /** Returns the number of degrees of freedom associated with this Node*/
                int getDOF()const;
                
}; // class RigidBodyNode

///////////////////////////////////////////////////////

inline int RigidBodyNode::getDOF()const{
        // Note: number of u's associated wiht any joint is equal to the 
        // degrees of freedom of the system.
        return data_->getDOF();
}

///////////////////////////////////////////////////////

inline void RigidBodyNode::sweepBackwardRNEA(const int& ChildID, State& state){
        this->data_->sweepBackwardRNEA(ChildID,this->Parent_,state);	
}

///////////////////////////////////////////////////////

inline void RigidBodyNode::sweepForwardRNEA(const SystemAcceleration *SysAccl,const int& ChildID, State& state){
        this->data_->sweepForwardRNEA(this->Parent_,  ChildID, SysAccl,this,state);		
}

///////////////////////////////////////////////////////

inline RigidBodyNode::RigidBodyNode(const int& NodeId, const RigidBody& data){
        Parent_ = NULL;
        data_ = new RigidBody(data);
        NodeId_ = NodeId;
}

///////////////////////////////////////////////////////

inline RigidBodyNode::RigidBodyNode(RigidBodyNode *ParentNode, const int& NodeId, const RigidBody& data){
        Parent_ = ParentNode;
        NodeId_ = NodeId;
        data_ = new RigidBody(data);
}

///////////////////////////////////////////////////////

inline RigidBodyNode::~RigidBodyNode(){
        delete data_;
}

inline RigidBodyNode::RigidBodyNode():Parent_(NULL),NodeId_(0){}

///////////////////////////////////////////////////////

inline void RigidBodyNode::sweepBackwardsABA(const int& ChildID, State& state){
        this->data_->sweepBackwardsABA(this, ChildID, this->Parent_,state);		
}

///////////////////////////////////////////////////////

inline int RigidBodyNode::getNodeId() const{
        return NodeId_;
}

///////////////////////////////////////////////////////

inline void RigidBodyNode::updQ(const double *q, State& state)const{
        state.updQ(q,NodeId_);
}

///////////////////////////////////////////////////////

inline void RigidBodyNode::updU(const double *u, State& state)const{
        state.updU(u,NodeId_);
}

///////////////////////////////////////////////////////

inline void RigidBodyNode::updJointTransform(const double *bodyq){
        this->data_->updJointTransform(bodyq, Parent_);
}

///////////////////////////////////////////////////////

inline void RigidBodyNode::initBetaK(const double *bodyq){
        this->data_->initBetaK(bodyq, Parent_);
}

///////////////////////////////////////////////////////

} // namespace RSIM
#endif